﻿
package bsplib;

class Matrix4A 
{
    public var m11 : Float;
    public var m12 : Float;
    public var m13 : Float;

    public var m21 : Float;
    public var m22 : Float;
    public var m23 : Float;

    public var m31 : Float;
    public var m32 : Float;
    public var m33 : Float;

    public var m41 : Float;
    public var m42 : Float;
    public var m43 : Float;

    public function new() {}
    
    private static function f( v : Float ) {
		var neg;
		if( v < 0 ) {
			neg = -1.0;
			v = -v;
		} else
			neg = 1.0;
		var digits = Std.int(4 - Math.log(v) / Math.log(10));
		if( digits < 1 )
			digits = 1;
		else if( digits >= 10 )
			return 0.;
		var exp = Math.pow(10,digits);
		return Math.floor(v * exp + .49999) * neg / exp;
	}
    
    public inline function toString() {
		return "< Matrix4A : \n" +
			"  < " + f(m11) + ", " + f(m21) + ", " + f(m31) + ", " + f(m41) + " >\n" +
            "  < " + f(m12) + ", " + f(m22) + ", " + f(m32) + ", " + f(m42) + " >\n" +
            "  < " + f(m13) + ", " + f(m23) + ", " + f(m33) + ", " + f(m43) + " >\n" +
		">";
	}
    
    public inline function clone() : Matrix4A {
        return new Matrix4A().copy(this);
    }
    
    public inline function copy( v : Matrix4A ) : Matrix4A {
        m11 = v.m11; m12 = v.m12; m13 = v.m13;
        m21 = v.m21; m22 = v.m22; m23 = v.m23;
        m31 = v.m31; m32 = v.m32; m33 = v.m33;
        m41 = v.m41; m42 = v.m42; m43 = v.m43;
        return this;
    }
    
    public inline function cIdentityMat () : Matrix4A {
        m11 = 1; m12 = 0; m13 = 0;
        m21 = 0; m22 = 1; m23 = 0;
        m31 = 0; m32 = 0; m33 = 1;
        m41 = 0; m42 = 0; m43 = 0;
        return this;
    }
    
    public inline function cZeroMat() : Matrix4A {
        m11 = 0; m12 = 0; m13 = 0;
        m21 = 0; m22 = 0; m23 = 0;
        m31 = 0; m32 = 0; m33 = 0;
        m41 = 0; m42 = 0; m43 = 0;
        return this;
    }
    
    public inline function cTranslationMat( x : Float, y : Float, z : Float ) : Matrix4A {
        m11 = 1; m12 = 0; m13 = 0;
        m21 = 0; m22 = 1; m23 = 0;
        m31 = 0; m32 = 0; m33 = 1;
        m41 = x; m42 = y; m43 = z;
        return this;
    }
    
    public inline function translate( x : Float, y : Float, z : Float ) : Matrix4A {
        m41 += x * m11 + y * m21 + z * m31;
        m42 += x * m12 + y * m22 + z * m32;
        m43 += x * m13 + y * m23 + z * m33;
		return this;
    }
    
    public inline function cTranslate( A : Matrix4A, x : Float, y : Float, z : Float ) : Matrix4A {
        m11 = A.m11; m12 = A.m12; m13 = A.m13;
        m21 = A.m21; m22 = A.m22; m23 = A.m23;
        m31 = A.m31; m32 = A.m32; m33 = A.m33;
        
        m41 = A.m41 + x * A.m11 + y * A.m21 + z * A.m31;
        m42 = A.m42 + x * A.m12 + y * A.m22 + z * A.m32;
        m43 = A.m43 + x * A.m13 + y * A.m23 + z * A.m33;
        return this;
    }
    
    public inline function cScaleMat( x : Float, y : Float, z : Float ) : Matrix4A {
        m11 = x; m12 = 0; m13 = 0;
        m21 = 0; m22 = y; m23 = 0;
        m31 = 0; m32 = 0; m33 = z;
        m41 = 0; m42 = 0; m43 = 0;
        return this;
    }
    
    public inline function scale( x : Float, y : Float, z : Float ) : Matrix4A {
        m11 *= x; m12 *= x; m13 *= x;
        m21 *= y; m22 *= y; m23 *= y;
        m31 *= z; m32 *= z; m33 *= z;
        return this;
    }
    
    public inline function cScale( A : Matrix4A, x : Float, y : Float, z : Float ) : Matrix4A {
        m11 = A.m11 * x; m12 = A.m12 * x; m13 = A.m13 * x;
        m21 = A.m21 * y; m22 = A.m22 * y; m23 = A.m23 * y;
        m31 = A.m31 * z; m32 = A.m32 * z; m33 = A.m33 * z;
        m41 = A.m41; m42 = A.m42; m43 = A.m43;
        return this;
    }
    
    public inline function cRotationMat( radians : Float, x : Float, y : Float, z : Float ) : Matrix4A {
        Debug.assert( Math.abs( 1.0 - Math.sqrt(x * x + y * y + z * z) ) < 0.00001 );
        
		var c = Math.cos( radians );
		var s = Math.sin( radians );
		var ci = 1-c;
		
		var sx = s*x;
		var sy = s*y;
		var sz = s*z;
		
		var cxy = x*y*ci;
		var cyz = y*z*ci;
		var cxz = x*z*ci;
		
		m11 = x*x*ci+c;
		m21 = cxy-sz;
		m31 = cxz + sy;
		m12 = cxy+sz;
		m22 = y*y*ci+c;
		m32 = cyz-sx;
		m13 = cxz-sy;
		m23 = cyz+sx;
		m33 = z*z*ci+c;
		m41 = 0;
		m42 = 0;
		m43 = 0;
        return this;
    }
    
    public inline function rotate( radians : Float, x : Float, y : Float, z : Float ) : Matrix4A {
        Debug.assert( Math.abs( 1 - Math.sqrt(x * x + y * y + z * z) ) < 0.00001 );
        
		var c = Math.cos( radians );
		var s = Math.sin( radians );
		var ci = 1-c;
		
		var sx = s*x;
		var sy = s*y;
		var sz = s*z;
		
		var cxy = x*y*ci;
		var cyz = y*z*ci;
		var cxz = x*z*ci;
		
		var a11 = x*x*ci+c;
		var a21 = cxy-sz;
		var a31 = cxz + sy;
		var a12 = cxy+sz;
		var a22 = y*y*ci+c;
		var a32 = cyz-sx;
		var a13 = cxz-sy;
		var a23 = cyz+sx;
		var a33 = z*z*ci+c;
		    
        var t11 = a11 * m11 + a12 * m21 + a13 * m31;
        var t12 = a11 * m12 + a12 * m22 + a13 * m32;
        var t13 = a11 * m13 + a12 * m23 + a13 * m33;
        var t21 = a21 * m11 + a22 * m21 + a23 * m31;
        var t22 = a21 * m12 + a22 * m22 + a23 * m32;
        var t23 = a21 * m13 + a22 * m23 + a23 * m33;
        m31 = a31 * m11 + a32 * m21 + a33 * m31;
        m32 = a31 * m12 + a32 * m22 + a33 * m32;
        m33 = a31 * m13 + a32 * m23 + a33 * m33;
        
        m11 = t11; m12 = t12; m13 = t13;
        m21 = t21; m22 = t22; m23 = t23;
        
        return this;
    }
    
    public inline function cRotate( A : Matrix4A,  radians : Float, x : Float, y : Float, z : Float ) : Matrix4A {
        // TODO: Assert x y z is a normalized vector
		var c = Math.cos( radians );
		var s = Math.sin( radians );
		var ci = 1-c;
		
		var sx = s*x;
		var sy = s*y;
		var sz = s*z;
		
		var cxy = x*y*ci;
		var cyz = y*z*ci;
		var cxz = x*z*ci;
		
		var a11 = x*x*ci+c;
		var a21 = cxy-sz;
		var a31 = cxz + sy;
		var a12 = cxy+sz;
		var a22 = y*y*ci+c;
		var a32 = cyz-sx;
		var a13 = cxz-sy;
		var a23 = cyz+sx;
		var a33 = z*z*ci+c;
		    
        m11 = a11 * A.m11 + a12 * A.m21 + a13 * A.m31;
        m12 = a11 * A.m12 + a12 * A.m22 + a13 * A.m32;
        m13 = a11 * A.m13 + a12 * A.m23 + a13 * A.m33;
        m21 = a21 * A.m11 + a22 * A.m21 + a23 * A.m31;
        m22 = a21 * A.m12 + a22 * A.m22 + a23 * A.m32;
        m23 = a21 * A.m13 + a22 * A.m23 + a23 * A.m33;
        m31 = a31 * A.m11 + a32 * A.m21 + a33 * A.m31;
        m32 = a31 * A.m12 + a32 * A.m22 + a33 * A.m32;
        m33 = a31 * A.m13 + a32 * A.m23 + a33 * A.m33;
        m41 = A.m41;
        m42 = A.m42;
        m43 = A.m43;
        
        return this;
    }
    
    public inline function mul( A : Matrix4A ) : Matrix4A {
        var t11 = A.m11 * m11 + A.m12 * m21 + A.m13 * m31;
        var t12 = A.m11 * m12 + A.m12 * m22 + A.m13 * m32;
        var t13 = A.m11 * m13 + A.m12 * m23 + A.m13 * m33;
        var t21 = A.m21 * m11 + A.m22 * m21 + A.m23 * m31;
        var t22 = A.m21 * m12 + A.m22 * m22 + A.m23 * m32;
        var t23 = A.m21 * m13 + A.m22 * m23 + A.m23 * m33;
        var t31 = A.m31 * m11 + A.m32 * m21 + A.m33 * m31;
        var t32 = A.m31 * m12 + A.m32 * m22 + A.m33 * m32;
        var t33 = A.m31 * m13 + A.m32 * m23 + A.m33 * m33;
        m41 = A.m41 * m11 + A.m42 * m21 + A.m43 * m31 + m41;
        m42 = A.m41 * m12 + A.m42 * m22 + A.m43 * m32 + m42;
        m43 = A.m41 * m13 + A.m42 * m23 + A.m43 * m33 + m43;
        
        m11 = t11; m12 = t12; m13 = t13;
        m21 = t21; m22 = t22; m23 = t23;
        m31 = t31; m32 = t32; m33 = t33;
        return this;
    }
    
    public inline function cMul( A : Matrix4A, B : Matrix4A ) : Matrix4A {
        m11 = B.m11 * A.m11 + B.m12 * A.m21 + B.m13 * A.m31;
        m12 = B.m11 * A.m12 + B.m12 * A.m22 + B.m13 * A.m32;
        m13 = B.m11 * A.m13 + B.m12 * A.m23 + B.m13 * A.m33;
        m21 = B.m21 * A.m11 + B.m22 * A.m21 + B.m23 * A.m31;
        m22 = B.m21 * A.m12 + B.m22 * A.m22 + B.m23 * A.m32;
        m23 = B.m21 * A.m13 + B.m22 * A.m23 + B.m23 * A.m33;
        m31 = B.m31 * A.m11 + B.m32 * A.m21 + B.m33 * A.m31;
        m32 = B.m31 * A.m12 + B.m32 * A.m22 + B.m33 * A.m32;
        m33 = B.m31 * A.m13 + B.m32 * A.m23 + B.m33 * A.m33;
        m41 = B.m41 * A.m11 + B.m42 * A.m21 + B.m43 * A.m31 + A.m41;
        m42 = B.m41 * A.m12 + B.m42 * A.m22 + B.m43 * A.m32 + A.m42;
        m43 = B.m41 * A.m13 + B.m42 * A.m23 + B.m43 * A.m33 + A.m43;
		return this;
    }
    
    public inline function cInverse( A : Matrix4A ) : Matrix4A {
		// slightly modified haXe3D implementation:
		m11 = A.m22*A.m33 - A.m23*A.m32;
		m12 = A.m13*A.m32 - A.m12*A.m33;
		m13 = A.m12*A.m23 - A.m13*A.m22;
		m21 = A.m23*A.m31 - A.m21*A.m33;
		m22 = A.m11*A.m33 - A.m13*A.m31;
		m23 = A.m13*A.m21 - A.m11*A.m23;
		m31 = A.m21*A.m32 - A.m22*A.m31;
		m32 = A.m12*A.m31 - A.m11*A.m32;
		m33 = A.m11*A.m22 - A.m12*A.m21;

		var det = A.m11 * m11 + A.m12 * m21 + A.m13 * m31;
        // TODO: Check det > epsilon
		var invDet = 1.0 / det;
		m11 *= invDet; m12 *= invDet; m13 *= invDet;
		m21 *= invDet; m22 *= invDet; m23 *= invDet;
		m31 *= invDet; m32 *= invDet; m33 *= invDet;
        
        m41 = -( A.m41 * m11 + A.m42 * m21 + A.m43 * m31 );
        m42 = -( A.m41 * m12 + A.m42 * m22 + A.m43 * m32 );
        m43 = -( A.m41 * m13 + A.m42 * m23 + A.m43 * m33 );
        
        return this;
	}
    
    public inline function invert() : Matrix4A {
        // slightly modified haXe3D implementation:
        var t11 = m11; var t12 = m12; var t13 = m13;
		var t21 = m21; var t22 = m22;//var t23 = m23;
		var t31 = m31;//var t32 = m32; var t33 = m33;
		m11 = t22*m33 - m23*m32;
		m12 = t13*m32 - t12*m33;
		m13 = t12*m23 - t13*t22;
		m21 = m23*t31 - t21*m33;
		m22 = t11*m33 - t13*t31;
		m23 = t13*t21 - t11*m23;
		m31 = t21*m32 - t22*t31;
		m32 = t12*t31 - t11*m32;
		m33 = t11*t22 - t12*t21;
		var det = t11 * m11 + t12 * m21 + t13 * m31;
        // TODO: Check det > epsilon
		var invDet = 1.0 / det;
		m11 *= invDet; m12 *= invDet; m13 *= invDet;
		m21 *= invDet; m22 *= invDet; m23 *= invDet;
		m31 *= invDet; m32 *= invDet; m33 *= invDet;
        
        var t41 = m41; var t42 = m42;
        m41 = -( t41 * m11 + t42 * m21 + m43 * m31 );
        m42 = -( t41 * m12 + t42 * m22 + m43 * m32 );
        m43 = -( t41 * m13 + t42 * m23 + m43 * m33 );
        
        return this;
    }
    
    public inline function transformVec3( src : Vec3f, dst : Vec3f ) : Vec3f {
        var tmp_x = src.x; var tmp_y = src.y;
        dst.x = tmp_x * m11 + tmp_y * m21 + src.z * m31 + m41;
        dst.y = tmp_x * m12 + tmp_y * m22 + src.z * m32 + m42;
        dst.z = tmp_x * m13 + tmp_y * m23 + src.z * m33 + m43;
        return dst;
    }
    
}